#pragma config(Sensor, in1,    right,               sensorReflection)
#pragma config(Sensor, in2,    middle,              sensorReflection)
#pragma config(Sensor, in3,    left,                sensorReflection)
#pragma config(Sensor, in4,    back,                sensorReflection)
#pragma config(Sensor, in5,    sonar,               sensorSONAR, int1)
#pragma config(Motor,  port2,           motor1,        tmotorNormal, openLoop)
#pragma config(Motor,  port3,           motor2,        tmotorNormal, openLoop)
#pragma config(Motor,  port4,           motor3,        tmotorNormal, openLoop)
#pragma config(Motor,  port5,           motor4,        tmotorNormal, openLoop)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

/*  This is called prototyping.
	I list all my methods and parameter2
	here so when I create them the ordewr doesn't matter.*/
void radioPrep();
void radioControl();
void checkLights();
void isBall();
void amIOnTheLine();
void goBabyGo(int speed);
void turnLeft(int speed);
void turnRight(int speed);


int radio = 0; //holds the input from the radio controller. Currently used to break loops.
int lightLeft = 0; // holds the value for the left most light sensor
int lightMiddle = 0; // holds the value for the middle light sensor
int lightRight = 0; // holds the value for the right most light sensor
int ball = 0; // holds the value of the touch sensor
int turnSpeed = 45; // turnSpeed needs to be less than speed to keep from overcorrecting on turns
int cruiseSpeed = 85; // This is the main cruise speed for the line follower

/* This is the main method where all the fun stuff happens*/
task main()
{
  radioPrep();
  while(true)
  {
    radio = 0; // insures correct value in radio
    isBall();
	  /*While in this loop the robot is radio controlled*/
	  while(radio == 0)
	  {
	    radioControl();
	    isBall();
	    radio = vexRT(Ch5); // updates the radio value
	  }
	  radio = 0; // insures correct value in radio
	  wait1Msec(500); // This is needed to allow the user time to release the radio button
	  while(radio == 0)
	  {
	    radio = vexRT(Ch5);
	    goBabyGo(cruiseSpeed);
	    checkLights();
	    isBall();
	    amIOnTheLine();
	  }
	  wait1Msec(500); // This is needed to allow the user time to release the radio button
  }
}
/*  This method sets the motors to their correct position
	and sets the robot to accept radio controll*/
void radioPrep()
{
  bMotorFlippedMode[port3] = 1;
  bMotorFlippedMode[port5] = 1;
  bVexAutonomousMode = false;
}
/* Updates all the motor speeds from the radio controll*/
void radioControl()
{
    motor[port2] = vexRT(Ch2);
    motor[port3] = vexRT(Ch3);
    motor[port4] = vexRT(Ch2);
    motor[port5] = vexRT(Ch3);
}
/* Updates the values for all the light sensors*/
void checkLights()
{
   lightLeft = SensorValue(left);
   lightMiddle = SensorValue(middle);
   lightRight = SensorValue(right);
   isBall();
}
/*  Checks to see if the robot is moving off the line
	then corrects it's trajectory. This is set to work
	on a dark line contrasted against a bright surface.*/
void amIOnTheLine()
{
  while(((lightMiddle - lightLeft) < (lightRight - lightLeft))&& radio == 0)
    {
         radio = vexRT(Ch5);
         checkLights();
         isBall();
         turnLeft(turnSpeed);

    }
    while(((lightMiddle - lightRight) < (lightLeft - lightRight)) && radio == 0)
    {
         radio = vexRT(Ch5);
         checkLights();
         isBall();
         turnRight(turnSpeed);

    }

}

void isBall()
{
  ball = SensorValue(sonar);
  if(ball == 5)
  {
    turnRight(127);
    wait1Msec(1600);
    ball = 0;
  }

}

/* This method sets all the motors to their cruise speed*/
void goBabyGo(int speed)
{
  motor[port2]= speed;
  motor[port3]= speed;
  motor[port4]= speed;
  motor[port5]= speed;
}
/* This method turns the robot to the left*/
void turnLeft(int speed)
{
  motor[port2] = -speed;
  motor[port3] = speed;
  motor[port4] = -speed;
  motor[port5] = speed;
}
/* This method turns the robot to the right*/
void turnRight(int speed)
{
  motor[port2] = speed;
  motor[port3] = -speed;
  motor[port4] = speed;
  motor[port5] = -speed;
}
